﻿#include "Travel.h"
#include "Common.h"
TravelManipulator::TravelManipulator()
	: m_vPosition(12.0f, 0.0f, 10.0f),
	m_vPersonPosition(0.0f, 0.0f, 4.0f)
	, m_vRotation(osg::PI_2, 0.0f, 0.0f),	
	m_vStep(2),
	m_vRotateStep(0.0),
	m_iLeftX(0),m_iLeftY(0),
	m_bLeftDown(false),
	m_node(nullptr),
	m_nCurrentNode(nullptr),
	m_bAuto(false)

{
}


void TravelManipulator::setByMatrix(const osg::Matrixd& matrix)
{
}

void TravelManipulator::setByInverseMatrix(const osg::Matrixd& matrix)
{
}

osg::Matrixd TravelManipulator::getMatrix() const
{
	osg::Matrixd mat;
	mat.makeTranslate(m_vPosition);
	return osg::Matrixd::rotate(m_vRotation[0], osg::X_AXIS, m_vRotation[1], osg::Y_AXIS, m_vRotation[2], osg::Z_AXIS) * mat; // osg乘法从左向右应用。
}

// 这个返回的就是ViewMatrix
osg::Matrixd TravelManipulator::getInverseMatrix() const
{
	osg::Matrixd mat;
	mat.makeTranslate(m_vPosition);
	return osg::Matrixd::inverse(osg::Matrixd::rotate(m_vRotation[0], osg::X_AXIS, m_vRotation[1], osg::Y_AXIS, m_vRotation[2], osg::Z_AXIS) * mat); // osg乘法从左向右应用。
}

bool TravelManipulator::handle(osgGA::Event* event, osg::Object* object, osg::NodeVisitor* nv)
{
	osgGA::GUIEventAdapter* ea = dynamic_cast<osgGA::GUIEventAdapter*>(event);

	//osgViewer::Viewer* view = dynamic_cast<osgViewer::Viewer*>(object);
	//if (view)
	//{
	//	m_node = view->getSceneData();
	//}

	switch (ea->getEventType())
	{
	case osgGA::GUIEventAdapter::KEYDOWN:
	{
		if ((ea->getKey() == 'w') || (ea->getKey() == 'W') || (ea->getKey() == osgGA::GUIEventAdapter::KEY_Up)) {
			ChangePosition(osg::Vec3d(m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), 0.0f));
			return true;
		}
		else if (ea->getKey() == 's' || (ea->getKey() == 'S') || (ea->getKey() == osgGA::GUIEventAdapter::KEY_Down)) {
			ChangePosition(osg::Vec3d(-m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), -m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), 0.0f));
			return true;
		}
		else if (ea->getKey() == 'a' || ea->getKey() == 'A') {
			ChangePosition(osg::Vec3d(-m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), 0.0f));
			return true;
		}
		else if (ea->getKey() == 'd' || ea->getKey() == 'D') {
			ChangePosition(osg::Vec3d(m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), -m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), 0.0f));
			return true;
		}
		else if (ea->getKey() == osgGA::GUIEventAdapter::KEY_Left) {
			m_vRotation[2] += 0.2;
			return true;
		}
		else if (ea->getKey() == osgGA::GUIEventAdapter::KEY_Right) {
			m_vRotation[2] -= 0.2;
			return true;
		}
		else if (ea->getKey() == osgGA::GUIEventAdapter::KEY_Home) {
			ChangePosition(osg::Vec3d(0, 0, m_vStep));
			return true;
		}
		else if (ea->getKey() == osgGA::GUIEventAdapter::KEY_End) {
			ChangePosition(osg::Vec3d(0, 0, -m_vStep));
			return true;
		}
	}
	case osgGA::GUIEventAdapter::PUSH:
	{
		if (ea->getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
		{
			m_iLeftX = ea->getX();
			m_iLeftY = ea->getY();
			m_bLeftDown = true;
		}
		return false;
	}
	case osgGA::GUIEventAdapter::DRAG:
	{
		if (m_bLeftDown)
		{
			int delX = ea->getX() - m_iLeftX;
			int delY = ea->getY() - m_iLeftY;
			m_vRotation[2] -= osg::DegreesToRadians(0.005 * delX);
			m_vRotation[0] += osg::DegreesToRadians(0.005 * delY);
			
			if (m_vRotation[0] <= 0)
			{
				m_vRotation[0] = 0;
			}
			if (m_vRotation[0] >= osg::PI)
			{
				m_vRotation[0] = osg::PI;
			}
		}
		break;
	}
	case osgGA::GUIEventAdapter::RELEASE:
	{
		if (ea->getKey() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
		{
			m_bLeftDown = false;
		}
		break;
	}
	}

	return false;
}

void TravelManipulator::setStep(int step)
{
	m_vStep = step;
}

int TravelManipulator::getStep()
{
	return m_vStep;
}

void TravelManipulator::setPosition(osg::Vec3d& position)
{
	m_vPosition = position;
}

void TravelManipulator::ChangePosition(osg::Vec3d& delta)
{
	// 源点m_vPosition与目的点newPos
	//osg::Vec3d personDelta = delta * 1.2;
	osg::Vec3d newPos = m_vPosition + delta;
	osg::Vec3d start = osg::Vec3d(newPos.x(), newPos.y(), newPos.z() + 1);
	osg::Vec3d end = osg::Vec3d(newPos.x(), newPos.y(), newPos.z() - 1000);
	osg::Vec3d start2 = osg::Vec3d(m_vPosition.x(), m_vPosition.y(), m_vPosition.z()+2);
	osg::Vec3d end2 = osg::Vec3d(newPos.x(), newPos.y(), newPos.z() + 2);

	osg::ref_ptr<osgUtil::IntersectionVisitor> iv = new osgUtil::IntersectionVisitor;
	osg::ref_ptr<osgUtil::LineSegmentIntersector> ls = new osgUtil::LineSegmentIntersector(start, end);  // 检查前进后的点的上下交点
	osg::ref_ptr<osgUtil::LineSegmentIntersector> ls2 = new osgUtil::LineSegmentIntersector(start2, end2); // 检查往前进方向走的路上是否有障碍物
	osg::ref_ptr<osgUtil::IntersectorGroup> lg = new osgUtil::IntersectorGroup;

	osgUtil::LineSegmentIntersector::Intersections intersections;
	long height = 0;

	// 一次能添加多个检测器
	lg->addIntersector(ls.get());
	lg->addIntersector(ls2.get());

	iv->setIntersector(lg.get());
	m_node->accept(*iv.get());

	if (ls->containsIntersections() && (!ls2->containsIntersections()))
	{
		// 取出交点
		intersections = ls->getIntersections();
		osgUtil::LineSegmentIntersector::Intersections::iterator iter = intersections.begin();
		height = (intersections.begin())->getWorldIntersectPoint().z();
		for (; iter != intersections.end(); iter++)
		{
			if (height < iter->getWorldIntersectPoint().z())
			{
				height = iter->getWorldIntersectPoint().z();
			}
		}
	}
	else
	{
		// 不许走
		return;
	}

	m_vPosition += delta;
	m_vPosition.set(osg::Vec3d(m_vPosition[0], m_vPosition[1], height+1));
	//std::cout << "position x:" << m_vPosition[0]
	//	<< " y: " << m_vPosition[1]
	//	<< " z: " << m_vPosition[2]
	//	<< std::endl;

	// m_vPersonPosition = m_vPosition + personDelta;
	if (m_nCurrentNode != nullptr)
	{
		//std::cout << "person position x:" << m_vPersonPosition[0] 
		//	<< " y: " << m_vPersonPosition[1] 
		//	<< " z: " << height
		//	<< std::endl;
		// m_nCurrentNode->toPosition(m_vPosition);
	}

}

void TravelManipulator::setNode(osg::Node* node)
{
	if (!m_bAuto)
	{
		m_node = node;
	}
}

void TravelManipulator::setAuto(bool bauto)
{
	m_bAuto = bauto;
}

void TravelManipulator::setCurrentNode(NodeMatrix* node)
{
	m_nCurrentNode = node;
}











